
from visualiser import VisualPart, Visualiser
from simulator import PhysicalPart, Simulator
from time import sleep


class RobotNode:
	def __init__(self, name):
		self.subnodes = {}
		self._name = name
	
	def add(self, node):
		self.subnodes[node.name()] = node

	def visual_parts(self):
		visual_parts = {}
		for node_name in self.subnodes:
			subnodes = self.subnodes[node_name].visual_parts()
			for subnode_name in subnodes:
				visual_parts[subnode_name] = subnodes[subnode_name]
		return visual_parts
	
	def physical_parts(self):
		physical_parts = {}
		for node_name in self.subnodes:
			subnodes = self.subnodes[node_name].physical_parts()
			for subnode_name in subnodes:
				physical_parts[subnode_name] = subnodes[subnode_name]
		return physical_parts
	
	def name(self):
		return self._name


class RobotPart:
	def __init__(self, name, physical_part, visual_part):
		self._name = name
		self._visual_part = visual_part
		self._physical_part = physical_part
	
	def visual_parts(self):
		return {self._name: self._visual_part}

	def physical_parts(self):
		return {self._name: self._physical_part}
		
	def name(self):
		return self._name


class RobotModel:
	def __init__(self):
		
		root = RobotNode("root")
		
		rootpath = "../cad/stl/"
		root.add(RobotPart("wheel", 
						   PhysicalPart(position = [0.0, 0.107, 0.0],
									    rotation = [0.0, 0.0, 0.0],
									    shape = "cylinder", 
									    radius = 0.06,
									    length = 0.0025,
									    material = "polystyrene"),
						   VisualPart(filename = rootpath+"wheel.stl", 
								      color = [0.8, 0.0, 0.0, 1.0], 
								      position = [0.0, 0.0, -0.00125],
								      rotation = [0.0, 0.0, 0.0])))
		
		"""
		root.add(RobotPart("hub1", None, VisualPart(filename = rootpath+"hub.stl", 
													color = [0.0, 0.0, 0.8, 1.0], 
													position = [0.0, 0.06, 0.005],
													rotation = [0.0, 0.0, 0.0])))
		root.add(RobotPart("hub2", None, VisualPart(filename = rootpath+"hub.stl", 
													color = [0.0, 0.0, 0.8, 1.0], 
													position = [0.0, 0.06, -0.01],
													rotation = [0.0, 0.0, 0.0])))
		root.add(RobotPart("motor1", None, VisualPart(filename = rootpath+"motor.stl", 
													color = [0.0, 0.8, 0.0, 1.0], 
													position = [0.0, 0.14, -0.03],
													rotation = [0.0, 0.0, 0.0])))
		root.add(RobotPart("motor2", None, VisualPart(filename = rootpath+"motor.stl", 
													color = [0.0, 0.8, 0.0, 1.0], 
													position = [0.0, 0.22, 0.0],
													rotation = [-90.0, 0.0, 0.0])))
		root.add(RobotPart("base", None, VisualPart(filename = rootpath+"base.stl", 
													color = [0.8, 0.8, 0.0, 1.0], 
													position = [0.0, 0.28, 0.0],
													rotation = [90.0, 90.0, 0.0])))
		"""
		
		visualiser = Visualiser(root.visual_parts())
		#visualiser.start()
		
		simulator = Simulator(root.physical_parts())
		simulator.start()
		sleep(1.0)
		print "will stop"
		simulator.stop()
		
		#visualiser.move("wheel", [0.0, 0.06, 0.0])
		
		

if __name__ == "__main__":
	robot_model = RobotModel()





